﻿using System;
using Microsoft.Xna.Framework;
using ShevaEngine.Core.Foundation;
using ShevaEngine.Core.Modules.ArtificialIntelligence.Pathfinding;
using ShevaEngine.Core.Modules.Scenes;

namespace ShevaEngine.Core.Modules.ArtificialIntelligence.Tasks
{
    /// <summary>
    /// MoveTo task.
    /// </summary>
    public class MoveToTask : AITask
    {
        /// <summary>MoveTo position.</summary>
        private Vector3 MoveToPosition;
        /// <summary>Navigation graph.</summary>
        private NavigationGraph NavigationGraph;
        /// <summary>Navigation path.</summary>
        public NavigationPath NavigationPath;
        /// <summary>Direction.</summary>
        public Vector3 Direction;
        /// <summary>Speed.</summary>
        public float Speed;
        /// <summary>Angle speed.</summary>
        public float AngleSpeed;
        

        /// <summary>
        /// Move to task.
        /// </summary>
        /// <param name="sceneObject"></param>
        /// <param name="navigationGraph"></param>
        /// <param name="moveToPosition"></param>
        public MoveToTask(SceneObject sceneObject, NavigationGraph navigationGraph, Vector3 moveToPosition,
            float speed = 1.0f, float angleSpeed = 0.5f)
            : base(sceneObject)
        {
            this.MoveToPosition = moveToPosition;
            this.NavigationGraph = navigationGraph;
            this.NavigationPath = new NavigationPath();
            this.NavigationGraph.FindPath(this.SceneObject.World.Translation, this.MoveToPosition, ref this.NavigationPath);
            this.Speed = speed;
            this.AngleSpeed = angleSpeed;
        }

        /// <summary>
        /// Method Update.
        /// </summary>
        /// <param name="time"></param>
        public override void Update(GameTime time)
        {
            MoveToTask.Move(this.SceneObject, this.NavigationPath, this.Speed, this.AngleSpeed);   
        }

        /// <summary>
        /// Method moves entity.
        /// </summary>
        /// <param name="sceneObject"></param>
        /// <param name="navigationPath"></param>
        public static void Move(SceneObject sceneObject, NavigationPath navigationPath, float speed, float angleSpeed)
        {
            if (navigationPath.Count > 0)
            {
                if ((navigationPath.Peek().Center - sceneObject.World.Translation).LengthSquared() == 0)
                    navigationPath.Pop();
                
                if (navigationPath.Count > 0)
                {
                    Vector3 newPosition = sceneObject.World.Translation +
                        Vector3.Normalize(navigationPath.Peek().Center - sceneObject.World.Translation) * speed;

                    if (Vector3.DistanceSquared(sceneObject.World.Translation, navigationPath.Peek().Center) <
                        Vector3.DistanceSquared(sceneObject.World.Translation, newPosition))
                    {
                        sceneObject.World *= Matrix.CreateTranslation(navigationPath.Peek().Center - sceneObject.World.Translation);
                        navigationPath.Pop();
                    }
                    else
                    {
                        Matrix temp = Matrix.CreateTranslation(newPosition - sceneObject.World.Translation);

                        Vector3 tempRotY0 = Vector3.Transform(Vector3.Zero, sceneObject.World);
                        Vector3 tempRotY1 = Vector3.Transform(Vector3.UnitZ, sceneObject.World);

                        Vector3 tempRotY = Vector3.Normalize(tempRotY1 - tempRotY0);

                        float YAngle = MoveToTask.TurnToFace(navigationPath, sceneObject, -(float)Math.Atan2(tempRotY.X, tempRotY.Z), angleSpeed);

                        sceneObject.World =
                            Matrix.CreateRotationY(-YAngle) *
                            sceneObject.World *
                            Matrix.CreateTranslation(newPosition - sceneObject.World.Translation);
                    }
                }
            }
        }
       
        /// <summary>
        /// Calculates the angle that an object should face, given its position, its
        /// target's position, its current angle, and its maximum turning speed.
        /// </summary>
        private static float TurnToFace(NavigationPath navigationPath, SceneObject sceneObject, float currentAngle, float turnSpeed)
        {
            Vector3 xyz = navigationPath.Peek().Center - sceneObject.World.Translation;
            
            float desiredAngle = (float)Math.Atan2(xyz.Z, xyz.X);

            float difference = MathUtils.WrapAngle(desiredAngle - currentAngle);
            
            difference = MathHelper.Clamp(difference, -turnSpeed, turnSpeed);

            return MathUtils.WrapAngle(difference);
        }        
    }
}
